#include <iostream>
#include <cstring>      // 用于 std::strlen
#include <arpa/inet.h>  // Unix/Linux中的网络功能
#include <unistd.h>     // 包含close的声明
#include <sys/types.h>  // 包含socket的声明
#include <sys/socket.h> // 包含socket的声明
#include <netinet/in.h> // 包含sockaddr_in的声明
#include <random>
#include "../inc/fusion.hpp"
#include <cmath>
#include <math.h>
#include "runge_kutta.hpp"
#include <thread>
using namespace std;
using namespace faf;

float generateNoise(float min, float max)
{
    // 创建随机数生成器和分布
    std::random_device rd;                                        // 用于生成随机数的种子
    std::mt19937 gen(rd());                                       // 使用梅森旋转算法生成随机数
    std::uniform_real_distribution<float> distribution(min, max); // 生成在[min, max]之间的随机浮点数

    return distribution(gen); // 返回生成的随机数
}

    // 要发送的数据
    char data[100];
int thread1(void)
{
    // 线程1代码
    faf::fusion data_fusion(0.001,1,0);

    // 创建UDP套接字
    int sockfd = socket(AF_INET, SOCK_DGRAM, 0);
    if (sockfd < 0)
    {
        std::cerr << "创建套接字失败" << std::endl;
        return 1;
    }

    // 定义目标地址
    struct sockaddr_in servaddr;
    std::memset(&servaddr, 0, sizeof(servaddr));          // 将结构体清零
    servaddr.sin_family = AF_INET;                        // IPv4
    servaddr.sin_port = htons(8266);                      // 目标端口
    servaddr.sin_addr.s_addr = inet_addr("192.168.1.40"); // 目标IP地址



    for (int i = 0; i < 1000; i++)
    {

        
        float a = sin(i /1000.0*2*3.1415926) + generateNoise(0.0, 0.4);
        float b = sin(i /1000.0*2*3.1415926) + generateNoise(-0.4, 0.0);
        if (i%100 == 0)
        {
            a+=generateNoise(0, 1);
        }
        float out;

        out = data_fusion.EKF_Fusion(a, b);

        sprintf(data, "%f,%f,%f\n", a,b,out);
        // 发送数据
        ssize_t sentBytes = sendto(sockfd, data, std::strlen(data), 0,
                                   (struct sockaddr *)&servaddr, sizeof(servaddr));
        if (sentBytes < 0)
        {
            std::cerr << "发送数据失败" << std::endl;
            close(sockfd);
            return 1;
        }
        else
        {
            std::cout << "已发送 " << sentBytes << " 字节数据: " << data << std::endl;
        }
        usleep(1000); // 睡眠10ms
    }


    close(sockfd);

        // 关闭套接字


}

int thread2(void)
{

    faf::fusion data_fusion(0.01,1,0);

    // 创建UDP套接字
    int sockfd = socket(AF_INET, SOCK_DGRAM, 0);
    if (sockfd < 0)
    {
        std::cerr << "创建套接字失败" << std::endl;
        return 1;
    }

    // 定义目标地址
    struct sockaddr_in servaddr;
    std::memset(&servaddr, 0, sizeof(servaddr));          // 将结构体清零
    servaddr.sin_family = AF_INET;                        // IPv4
    servaddr.sin_port = htons(8266);                      // 目标端口
    servaddr.sin_addr.s_addr = inet_addr("192.168.1.16"); // 目标IP地址
  // 初始化
    Quaternion q = { 1, 0, 0, 0 }; // 初始四元数
    double wx = 0.0, wy = 0.0, wz = 0.3; // 角速度
    double ax = 0.0, ay = 0.0, az = 9.81; // 加速度
    double h = 0.01; // 时间步长
        Acceleration acc;
        acc.ax = ax;
        acc.ay = ay;
        acc.az = az;


    double exinc;//重力角度误差积分    
    double eyinc;
    double ezinc;


    // 更新四元数
    for (int i = 0; i < 100; i++) {
        wx = 0.0 + generateNoise(-0.1, 0);
        wy = 0.0 + generateNoise(-0.1, 0.1);
        wz = 0.0  ;

        double vx = 2 * (q.x * q.z - q.w * q.y);// 四元数在x轴上的分量
        double vy = 2 * (q.w * q.x + q.y * q.z);// 四元数在y轴上的分量
        double vz = (q.w * q.w) - (q.x * q.x) - (q.y * q.y) + (q.z * q.z); // 四元数在z轴上的分量




        //加速度归一化
        acc=normalizeacceleration(acc);
        
        //测得重力与计算所得重力的误差值     向量叉乘所得
        double ex = (ay*vz - az*vy);
        double ey = (az*vx - ax*vz);
        double ez = (ax*vy - ay*vx);
       
        double kp=0.6;
        double ki=0.1;


        exinc += ex*ki;
        eyinc += ey*ki;
        ezinc += ez*ki;

        wx =wx+ kp*ex+ exinc;
        wy =wy+ kp*ey+ eyinc;
        wz =wz+ kp*ez+ ezinc;
        

        q = updateQuaternion(q, wx, wy, wz, h);
        float pitch = asin(2 * (q.w * q.y - q.x * q.z));
        float roll = atan2(2 * (q.w * q.x + q.y * q.z), 1 - 2 * (q.y*q.y + q.z*q.z));
        float yaw = atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.x*q.x + q.y*q.y));

        sprintf(data, "%f,%f,%f,%f\n", q.w,q.x,q.y,q.z);
        // 发送数据
        ssize_t sentBytes = sendto(sockfd, data, std::strlen(data), 0,
                                   (struct sockaddr *)&servaddr, sizeof(servaddr));
        if (sentBytes < 0)
        {
            std::cerr << "发送数据失败" << std::endl;
            close(sockfd);
            return 1;
        }
        else
        {
            std::cout << "已发送 " << sentBytes << " 字节数据: " << data << std::endl;
        }
        usleep(1000); // 睡眠10ms
    }

}

int thread3(void)
{
    // 线程3代码

    //input data

    uint8_t input1[30][30]={
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,1,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        }  ;
    //预处理
    

    uint8_t input[30][30]={
        {0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
        {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0},
        {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0},
        {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0},
        {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0},
        {0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0},
        {0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0},
        {0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        }  ;
    //预处理


    //卷积核
    int kernel[3][3] = { {0,0, 1,},
                         {0,1,0,},
                         {1,0,0,}, };

        //二次卷积
    int kernel2[3][3] = { {0,-1,2,},
                         {-1,2,-1,},
                         {2,-1,0,}, };

    int width = 30; //数据源宽度
    int height = 30;//数据源高度  

    //步长
    int step = 2;

    int windwos_size = 3;


    int out_w=width/step;
    int out_h=height/step;

    //卷积

    int output[out_w][out_h];
    for (int i = 0; i < width; i +=step) 
    {
        for (int j = 0; j < height; j +=step) 
        {
            int sum = 0;
            for (int m = -1; m <=1; m++) 
            {
                for (int n = -1; n <=1; n++) 
                {
                    if (i+m < 0 || i+m >= width || j+n < 0 || j+n >= height) //超出边界
                    {
                       sum += 0;//其实什么都不干就可以了 但是为了明确思维，这里还是写了
                    }
                    else
                    {
                    sum += input1[i+m][j+n] * kernel[m+1][n+1];
                    }
                }
            }
        output[i/step][j/step]=sum;
        }
    }

    //激活
    for (int i = 0; i < out_w; i++) {
        for (int j = 0; j < out_h; j++) {
            if(output[i][j]>0)
            {
            output[i][j] = (output[i][j]);
            }
            else
            {
            output[i][j] = 0;
            }
        }
    }

    //输出
    for (int i = 0; i < out_w; i++) {
        for (int j = 0; j < out_h; j++) {
            std::cout << output[i][j] << " ";
        }
        std::cout << std::endl;
    }
std::cout << std::endl;

    //池化
    int pool_size = 2;
    int pool_out_w = out_w/pool_size;
    int pool_out_h = out_h/pool_size;
    int pool_output[pool_out_w][pool_out_h];
    for (int i = 0; i < pool_out_w; i++) {
        for (int j = 0; j < pool_out_h; j++) {
            int sum = 0;
            for (int m = 0; m < pool_size; m++)
            {
                for (int n = 0; n < pool_size; n++)
                {
                    sum = (sum>output[i*pool_size+m][j*pool_size+n])?sum:output[i*pool_size+m][j*pool_size+n];//取最大值
                }
            }
            pool_output[i][j] = sum;
        }
    }
    




    //步长
    int step2 = 2;
    int width2 = pool_out_w;
    int height2 = pool_out_h;
    int out_w2=width2/step2;
    int out_h2=height2/step2;
    //卷积

/*    0 0 0 0 0 0 0
        -
      0|1 1 1 1 1
      0|1 1 1 1 1
      0|1 1 1 1 1
      0
*/
    int output2[out_w2][out_h2];
    for (int i = 0; i < width2; i +=step2) 
    {
        for (int j = 0; j < height2; j +=step2) 
        {
            int sum = 0;
            for (int m = -1; m <=1; m++) 
            {
                for (int n = -1; n <=1; n++) 
                {
                    if (i+m < 0 || i+m >= width2 || j+n < 0 || j+n >= height2) //超出边界
                    {
                       sum += 0;//其实什么都不干就可以了 但是为了明确思维，这里还是写了
                    }
                    else
                    {
                    sum += pool_output[i+m][j+n] * kernel2[m+1][n+1];
                    }
                }
            }
        output2[i/step2][j/step2]=sum;
        }
    }
//激活
    for (int i = 0; i < out_w2; i++) {
        for (int j = 0; j < out_h2; j++) {
            if(output2[i][j]>0)
            {
            output2[i][j] = (output2[i][j]);
            }
            else
            {
            output2[i][j] = 0;
            }
        }
    }

//输出
int pp=0;
    for (int i = 0; i < out_w2; i++) {
        for (int j = 0; j < out_h2; j++) {
            std::cout << output2[i][j] << " ";
            pp+=output2[i][j];
        }
        std::cout << std::endl;
    }

std::cout << "拟合度 =" << (float)pp/9 *100<<"%" << std::endl;
    
}
int main()
{


    thread first(thread1);
    //thread second(thread2);

   // thread theread(thread3);
    std::cout << "主线程结束" << endl;
    std::cout << "启动子线程1" << endl;
    first.join();
    //second.join();
    //theread.join();
    std::cout << "子线程结束" << endl;
    return 0;
}